﻿/*
* Copyright (c) 2010-2012 Tesla Engine Group
* 
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
* 
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
* 
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/

using System;
using Tesla.Content;
using Tesla.Math;

namespace Tesla.Bounding {
    /// <summary>
    /// A sphere that is defined by a center and a radius. This is one of the simpler volumes and collision checks
    /// are fast, but may yield more false-positives than using volumes that more tightly enclose the geometry (such as OrientedBoundingBox).
    /// 
    /// The algorithm for generating the sphere is an implemention of Welzl's minimum-volume sphere algorithm.
    /// </summary>
    public sealed class BoundingSphere : BoundingVolume {
        private float m_radius;

        //For welzl calculations
        private const float RADIUS_EPSILON = 1.00001f;

        /// <summary>
        /// Gets or sets the radius of the sphere.
        /// </summary>
        public float Radius {
            get {
                return m_radius;
            }
            set {
                m_radius = value;
            }
        }

        /// <summary>
        /// Gets the volume of the sphere.
        /// </summary>
        public override float Volume {
            get {
                return (4.0f * MathHelper.OneThird * MathHelper.Pi * m_radius * m_radius * m_radius);
            }
        }

        /// <summary>
        /// Gets the bounding type enumeration.
        /// </summary>
        public override BoundingType BoundingType {
            get {
                return BoundingType.Sphere;
            }
        }

        /// <summary>
        /// Constructs a new bounding sphere centered at the origin with zero radius.
        /// </summary>
        public BoundingSphere() 
            : base() {
            m_radius = 0;
        }

        /// <summary>
        /// Constructs a new bounding sphere.
        /// </summary>
        /// <param name="center">Center of the sphere</param>
        /// <param name="radius">Radius of the sphere</param>
        public BoundingSphere(Vector3 center, float radius)
            : base(center) {
                m_radius = radius;
        }

        /// <summary>
        /// Constructs a new bounding sphere cloned from the source sphere.
        /// </summary>
        /// <param name="source">Sphere to clone</param>
        public BoundingSphere(BoundingSphere source) {
            m_center = source.Center;
            m_radius = source.Radius;
        }

        /// <summary>
        /// Sets the bounding sphere to copy from the source volume or to contain it.
        /// </summary>
        /// <param name="volume">Source volume</param>
        public override void Set(BoundingVolume volume) {
            switch(volume.BoundingType) {
                case BoundingType.AABB:
                    BoundingBox bb = volume as BoundingBox;
                    m_center = bb.Center;
                    m_radius = bb.Extents.Length();
                    break;
                case BoundingType.Sphere:
                    BoundingSphere sphere = volume as BoundingSphere;
                    m_center = sphere.Center;
                    m_radius = sphere.Radius;
                    break;
                case BoundingType.OBB:
                    //TODO
                    break;
            }
        }

        /// <summary>
        /// Sets the bounding sphere to the specified center and radius.
        /// </summary>
        /// <param name="center">New center of the sphere</param>
        /// <param name="radius">New radius of the sphere</param>
        public void Set(Vector3 center, float radius) {
            m_center = center;
            m_radius = radius;
        }

        /// <summary>
        /// Returns a new instance of BoundingSphere with the same center/radius.
        /// </summary>
        /// <returns></returns>
        public override BoundingVolume Clone() {
            return new BoundingSphere(this);
        }

        /// <summary>
        /// Computes the distance from the point to the nearest edge of the volume.
        /// </summary>
        /// <param name="point">Point to use</param>
        /// <returns>Distance from the point to the volume</returns>
        public override float DistanceToEdge(Vector3 point) {
            return Vector3.Distance(m_center, point) - m_radius;
        }

        /// <summary>
        /// Computes the bounding sphere from a collection of 3D points.
        /// 
        /// </summary>
        /// <param name="points">Collection of points</param>
        public override void ComputeFromPoints(Vector3[] points) {
            Vector3[] copy = new Vector3[points.Length];
            Array.Copy(points, copy, points.Length);
            CalculateWelzl(copy, copy.Length, 0, 0);
        }

        /// <summary>
        /// Computes the bounding sphere from a collection of indexed primitives.
        /// </summary>
        /// <param name="vertices">Collection of positions</param>
        /// <param name="indices">Collection of indices</param>
        public override void ComputeFromPrimitives(Vector3[] vertices, int[] indices) {
            Vector3[] copy = new Vector3[indices.Length];

            for(int i = 0; i < indices.Length; i++) {
                copy[i] = vertices[indices[i]];
            }

            CalculateWelzl(copy, copy.Length, 0, 0);
        }

        /// <summary>
        /// Computes the bounding sphere from a collection of indexed primitives.
        /// </summary>
        /// <param name="vertices">Collection of positions</param>
        /// <param name="indices">Collection of indices</param>
        public override void ComputeFromPrimitives(Vector3[] vertices, short[] indices) {
            Vector3[] copy = new Vector3[indices.Length];

            for(int i = 0; i < indices.Length; i++) {
                copy[i] = vertices[indices[i]];
            }

            CalculateWelzl(copy, copy.Length, 0, 0);
        }

        //Welzl minimum bounding sphere algorithm
        private void CalculateWelzl(Vector3[] points, int length, int supportCount, int index) {
            switch(supportCount) {
                case 0:
                    m_radius = 0;
                    m_center = Vector3.Zero;
                    break;
                case 1:
                    m_radius = 1.0f - RADIUS_EPSILON;
                    m_center = points[index-1];
                    break;
                case 2:
                    SetSphere(points[index-1], points[index-2]);
                    break;
                case 3:
                    SetSphere(points[index-1], points[index-2], points[index-3]);
                    break;
                case 4:
                    SetSphere(points[index-1], points[index-2], points[index-3], points[index-4]);
                    return;
            }
            for(int i = 0; i < length; i++) {
                Vector3 comp = points[i + index];
                float distSqr;
                Vector3.DistanceSquared(ref comp, ref m_center, out distSqr);
                if(distSqr - (m_radius * m_radius) > RADIUS_EPSILON - 1.0f) {
                    for(int j = i; j > 0; j--) {
                        Vector3 a = points[j + index];
                        Vector3 b = points[j - 1 + index];
                        points[j + index] = b;
                        points[j - 1 + index] = a;
                    }
                    CalculateWelzl(points, i, supportCount + 1, index + 1);
                }
            }
        }

        //For Welzl calc - 2 support points
        private void SetSphere(Vector3 O, Vector3 A) {
            Radius = (float) System.Math.Sqrt(((A.X - O.X) * (A.X - O.X) + (A.Y - O.Y)
                * (A.Y - O.Y) + (A.Z - O.Z) * (A.Z - O.Z)) / 4.0f) + RADIUS_EPSILON - 1.0f;
            float x = (1 - .5f) * O.X + .5f * A.X;
            float y = (1 - .5f) * O.Y + .5f * A.Y;
            float z = (1 - .5f) * O.Z + .5f * A.Z;
            SetCenter(x, y, z);
        }

        //For Welzl calc - 3 support points
        private void SetSphere(Vector3 O, Vector3 A, Vector3 B) {
            Vector3 a = A - O;
            Vector3 b = B - O;
            Vector3 aCrossB = Vector3.Cross(a, b);
            float denom = 2.0f * Vector3.Dot(aCrossB, aCrossB);
            if(denom == 0) {
                m_center = Vector3.Zero;
                m_radius = 0;
            } else {
                Vector3 o = ((Vector3.Cross(aCrossB, a) * b.LengthSquared()) 
                    + (Vector3.Cross(b, aCrossB) * a.LengthSquared())) / denom;
                m_radius = o.Length() * RADIUS_EPSILON;
                m_center = O + o;
            }
        }

        //For Welzl calc - 4 support points
        private void SetSphere(Vector3 O, Vector3 A, Vector3 B, Vector3 C) {
            Vector3 a = A - O;
            Vector3 b = B - O;
            Vector3 c = C - O;

            float denom = 2.0f * (a.X * (b.Y * c.Z - c.Y * b.Z) - b.X 
                * (a.Y * c.Z - c.Y * a.Z) + c.X * (a.Y * b.Z - b.Y * a.Z));
            if(denom == 0) {
                m_center = Vector3.Zero;
                m_radius = 0;
            } else {
                Vector3 o = ((Vector3.Cross(a, b) * c.LengthSquared()) 
                    + (Vector3.Cross(c, a) * b.LengthSquared())
                    + (Vector3.Cross(b, c) * a.LengthSquared())) / denom;
                m_radius = o.Length() * RADIUS_EPSILON;
                m_center = O + o;
            }
        }

        /// <summary>
        /// Tests if the bounding box intersects with this bounding sphere.
        /// </summary>
        /// <param name="box">Bounding box to test</param>
        /// <returns>True if they intersect</returns>
        public override bool Intersects(BoundingBox box) {
            if(box == null) {
                return false;
            }

            Vector3 bCenter = box.Center;
            Vector3 extents = box.Extents;

            if(System.Math.Abs(m_center.X - bCenter.X) < m_radius + extents.X
                && System.Math.Abs(m_center.Y - bCenter.Y) < m_radius + extents.Y
                && System.Math.Abs(m_center.Z - bCenter.Z) < m_radius + extents.Z) {
                return true;
            }

            return false;
        }

        /// <summary>
        /// Tests if the bounding sphere intersects with this bounding sphere.
        /// </summary>
        /// <param name="sphere">Bounding sphere to test</param>
        /// <returns>True if they intersect</returns>
        public override bool Intersects(BoundingSphere sphere) {
            if(sphere == null) {
                return false;
            }

            Vector3 diff;
            Vector3 sCenter = sphere.Center;
            Vector3.Subtract(ref m_center, ref sCenter, out diff);

            float radSum = sphere.Radius + m_radius;
            float dot;
            Vector3.Dot(ref diff, ref diff, out dot);

            return dot <= radSum * radSum;
        }

        /// <summary>
        /// Tests if the oriented bounding box intersects with this bounding sphere.
        /// </summary>
        /// <param name="obb">Oriented bounding box to test</param>
        /// <returns>True if they intersect</returns>
        public override bool Intersects(OrientedBoundingBox obb) {
            if(obb == null) {
                return false;
            }

            return obb.Intersects(this);
        }

        /// <summary>
        /// Tests if the ray intersects with this sphere.
        /// </summary>
        /// <param name="ray">Ray to test</param>
        /// <returns>True if the ray intersects the sphere</returns>
        public override bool Intersects(Ray ray) {
            //Test if the origin is inside the sphere
            Vector3 rOrigin = ray.Origin;
            Vector3 diff;
            Vector3.Subtract(ref rOrigin, ref m_center, out diff);
            float radSquared = m_radius * m_radius;

            float dot;
            Vector3.Dot(ref diff, ref diff, out dot);
            float a = dot - radSquared;

            if(a <= 0.0f) {
                return true;
            }

            //Outside sphere
            Vector3 rDir = ray.Direction;
            float b;
            Vector3.Dot(ref rDir, ref diff, out b);
            if(b >= 0.0f) {
                return false;
            }

            return b * b >= a;
        }

        /// <summary>
        /// Tests if the ray intersects with this sphere.
        /// </summary>
        /// <param name="ray">Ray to test</param>
        /// <param name="result">Bool to hold the result, true if they intersect</param>
        public override void Intersects(ref Ray ray, out bool result) {
            //Test if the origin is inside the sphere
            Vector3 rOrigin = ray.Origin;
            Vector3 diff;
            Vector3.Subtract(ref rOrigin, ref m_center, out diff);
            float radSquared = m_radius * m_radius;

            float dot;
            Vector3.Dot(ref diff, ref diff, out dot);
            float a = dot - radSquared;

            if(a <= 0.0f) {
                result = true;
                return;
            }

            //Outside sphere
            Vector3 rDir = ray.Direction;
            float b;
            Vector3.Dot(ref rDir, ref diff, out b);
            if(b >= 0.0f) {
                result = false;
                return;
            }

            result = b * b >= a;
        }

        /// <summary>
        /// Tests where on the sphere the ray intersects, if it does.
        /// </summary>
        /// <param name="ray">Ray to test</param>
        /// <returns>Intersection result</returns>
        public override BoundingIntersectionRecord IntersectsWhere(Ray ray) {
            Vector3 diff;
            Vector3 rOrigin = ray.Origin;
            Vector3 rDir = ray.Direction;
            Vector3.Subtract(ref rOrigin, ref m_center, out diff);
            float a, a1, discr, sqrt;
            Vector3.Dot(ref diff, ref diff, out a);
            a -= m_radius * m_radius;

            //Check if we're inside the sphere, if so then the result must have
            //one exit point
            if(a <= 0.0f) {
                Vector3.Dot(ref rDir, ref diff, out a1);
                discr = (a1 * a1) - a;
                sqrt = MathHelper.Sqrt(discr);

                //Calc the distance and exit point
                float dist = sqrt - a1;
                Vector3 p;
                Vector3.Multiply(ref rDir, dist, out p);
                Vector3.Add(ref p, ref rOrigin, out p);
                IntersectionRecord rec = new IntersectionRecord(p, dist);

                return new BoundingIntersectionRecord(rec);
            }

            Vector3.Dot(ref rDir, ref diff, out a1);
            if(a1 >= 0.0f) {
                //No intersections
                return new BoundingIntersectionRecord();
            }

            discr = a1 * a1 - a;
            if(discr < 0.0f) {
                //Two complex-valued roots, No intersections
                return new BoundingIntersectionRecord();
            } else if(discr >= MathHelper.ZeroTolerance) {
                //Two distinct real-valued roots, two intersections
                sqrt = MathHelper.Sqrt(discr);

                float dist1 = System.Math.Abs(-a1 - sqrt);
                float dist2 = System.Math.Abs(-a1 + sqrt);
                Vector3 p1;
                Vector3.Multiply(ref rDir, dist1, out p1);
                Vector3.Add(ref p1, ref rOrigin, out p1);
                IntersectionRecord rec1 = new IntersectionRecord(p1, dist1);

                Vector3 p2;
                Vector3.Multiply(ref rDir, dist2, out p2);
                Vector3.Add(ref p2, ref rOrigin, out p2);
                IntersectionRecord rec2 = new IntersectionRecord(p2, dist2);

                return new BoundingIntersectionRecord(rec1, rec2);
            }

            //The ray intersects the sphere at exactly one point.
            float dis = -a1;
            Vector3 point;
            Vector3.Multiply(ref rDir, dis, out point);
            Vector3.Add(ref point, ref rOrigin, out point);
            IntersectionRecord rec0 = new IntersectionRecord(point, dis);

            return new BoundingIntersectionRecord(rec0);
        }

        /// <summary>
        /// Tests where on the sphere the ray intersects, if it does.
        /// </summary>
        /// <param name="ray">Ray to test</param>
        /// <param name="result">The intersection result</param>
        public override void IntersectsWhere(ref Ray ray, out BoundingIntersectionRecord result) {
            Vector3 diff;
            Vector3 rOrigin = ray.Origin;
            Vector3 rDir = ray.Direction;
            Vector3.Subtract(ref rOrigin, ref m_center, out diff);
            float a, a1, discr, sqrt;
            Vector3.Dot(ref diff, ref diff, out a);
            a -= m_radius * m_radius;

            //Check if we're inside the sphere, if so then the result must have
            //one exit point
            if(a <= 0.0f) {
                Vector3.Dot(ref rDir, ref diff, out a1);
                discr = (a1 * a1) - a;
                sqrt = MathHelper.Sqrt(discr);

                //Calc the distance and exit point
                float dist = sqrt - a1;
                Vector3 p;
                Vector3.Multiply(ref rDir, dist, out p);
                Vector3.Add(ref p, ref rOrigin, out p);
                IntersectionRecord rec = new IntersectionRecord(p, dist);

                result = new BoundingIntersectionRecord(rec);
                return;
            }

            Vector3.Dot(ref rDir, ref diff, out a1);
            if(a1 >= 0.0f) {
                //No intersections
                result = new BoundingIntersectionRecord();
                return;
            }

            discr = a1 * a1 - a;
            if(discr < 0.0f) {
                //No intersections
                result = new BoundingIntersectionRecord();
                return;
            } else if(discr >= MathHelper.ZeroTolerance) {
                //Entry and exit exist
                sqrt = MathHelper.Sqrt(discr);

                float dist1 = System.Math.Abs(-a1 - sqrt);
                float dist2 = System.Math.Abs(-a1 + sqrt);
                Vector3 p1;
                Vector3.Multiply(ref rDir, dist1, out p1);
                Vector3.Add(ref p1, ref rOrigin, out p1);
                IntersectionRecord rec1 = new IntersectionRecord(p1, dist1);

                Vector3 p2;
                Vector3.Multiply(ref rDir, dist2, out p2);
                Vector3.Add(ref p2, ref rOrigin, out p2);
                IntersectionRecord rec2 = new IntersectionRecord(p2, dist2);

                result = new BoundingIntersectionRecord(rec1, rec2);
                return;
            }

            //The ray intersects the sphere at exactly one point.
            float dis = -a1;
            Vector3 point;
            Vector3.Multiply(ref rDir, dis, out point);
            Vector3.Add(ref point, ref rOrigin, out point);
            IntersectionRecord rec0 = new IntersectionRecord(point, dis);

            result = new BoundingIntersectionRecord(rec0);
            return;
        }

        /// <summary>
        /// Tests if the plane intersects with this bounding sphere, and if not
        /// which side the volume is on relative to the plane.
        /// </summary>
        /// <param name="plane">Plane to test</param>
        /// <returns>Plane intersection type</returns>
        public override PlaneIntersectionType Intersects(Plane plane) {
            float dotC;
            Plane.DotCoordinate(ref plane, ref m_center, out dotC);

            if(dotC > m_radius) {
                return PlaneIntersectionType.Front;
            } else if(dotC < -m_radius) {
                return PlaneIntersectionType.Back;
            }

            return PlaneIntersectionType.Intersects;
        }

        /// <summary>
        /// Tests if the plane intersects with this bounding sphere, and if not
        /// which side the volume is on relative to the plane.
        /// </summary>
        /// <param name="plane">Plane to test</param>
        /// <param name="result">Result, the plane intersection type</param>
        public override void Intersects(ref Plane plane, out PlaneIntersectionType result) {
            float dotC;
            Plane.DotCoordinate(ref plane, ref m_center, out dotC);

            if(dotC > m_radius) {
                result = PlaneIntersectionType.Front;
                return;
            } else if(dotC < -m_radius) {
                result = PlaneIntersectionType.Back;
                return;
            }

            result = PlaneIntersectionType.Intersects;
        }

        /// <summary>
        /// Tests if the specified point is inside this bounding sphere or not.
        /// </summary>
        /// <param name="point">Point to test</param>
        /// <returns>Inside or outside</returns>
        public override ContainmentType Contains(Vector3 point) {
            float dist;
            Vector3.DistanceSquared(ref m_center, ref point, out dist);
            if(dist < m_radius * m_radius) {
                return ContainmentType.Inside;
            } else {
                return ContainmentType.Outside;
            }
        }

        /// <summary>
        /// Determines if the AABB is contained within this bounding volume.
        /// </summary>
        /// <param name="box">AABB to test</param>
        /// <returns>Containment type</returns>
        public override ContainmentType Contains(BoundingBox box) {
            if(box == null) {
                return ContainmentType.Outside;
            }

            Vector3 bMin = box.Min;
            Vector3 bMax = box.Max;

            Vector3 temp;
            float dist;
            Vector3.Clamp(ref m_center, ref bMin, ref bMax, out temp);
            Vector3.Distance(ref m_center, ref temp, out dist);

            //Check if we're outside
            if(dist > (m_radius * m_radius)) {
                return ContainmentType.Outside;
            }

            //Or contained
            if(((bMin.X + m_radius) <= m_center.X)
                && (m_center.X <= (bMax.X - m_radius))
                && ((bMax.X - bMin.X) > m_radius)
                && ((bMin.Y + m_radius) <= m_center.Y)
                && (m_center.Y <= (bMax.Y - m_radius))
                && ((bMax.Y - bMin.Y) > m_radius)
                && ((bMin.Z + m_radius) <= m_center.Z)
                && (m_center.Z <= (bMax.Z - m_radius))
                && ((bMax.X - bMin.X) > m_radius)) {
                return ContainmentType.Inside;
            }

            //Otherwise we overlap
            return ContainmentType.Intersects;
        }

        /// <summary>
        /// Determines if the sphere is contained within this bounding volume.
        /// </summary>
        /// <param name="sphere">Sphere to test</param>
        /// <returns>Containment type</returns>
        public override ContainmentType Contains(BoundingSphere sphere) {
            if(sphere == null) {
                return ContainmentType.Outside;
            }

            float dist;
            Vector3 sCenter = sphere.Center;
            float sRad = sphere.Radius;

            Vector3.Distance(ref m_center, ref sCenter, out dist);

            if((m_radius + sRad) < dist) {
                return ContainmentType.Outside;
            } else if((m_radius - sRad) < dist) {
                return ContainmentType.Intersects;
            }

            return ContainmentType.Inside;
        }

        /// <summary>
        /// Determines if the oriented bounding box is contained within this bounding volume.
        /// </summary>
        /// <param name="obb">Oriented bounding box to test</param>
        /// <returns>Containment type</returns>
        public override ContainmentType Contains(OrientedBoundingBox obb) {
            if(obb == null) {
                return ContainmentType.Outside;
            }

            return obb.Contains(this);
        }

        /// <summary>
        /// Merges this bounding volume with a second one. The resulting bounding
        /// volume is stored locally and will contain the original volumes completely.
        /// </summary>
        /// <param name="volume">Volume to merge with</param>
        public override void MergeLocal(BoundingVolume volume) {
            if(volume == null) {
                return;
            }

            float radius;
            Vector3 center;

            switch(volume.BoundingType) {
                case BoundingType.AABB:
                    BoundingBox box = volume as BoundingBox;
                    radius = box.Extents.Length();
                    center = box.Center;
                    MergeSphere(ref radius, ref center, this);
                    return;
                case BoundingType.Sphere:
                    BoundingSphere sphere = volume as BoundingSphere;
                    radius  = sphere.Radius;
                    center = sphere.Center;
                    MergeSphere(ref radius, ref center, this);
                    return;
                case Bounding.BoundingType.OBB:
                    OrientedBoundingBox obb = volume as OrientedBoundingBox;
                    MergeOBB(obb, this);
                    return;
            }
        }

        /// <summary>
        /// Merges this bounding volume with a second one. The resulting
        /// bounding volume is returned as a new object and will contain the 
        /// original volumes completely. The returned value will be of the
        /// same bounding type as the caller.
        /// </summary>
        /// <param name="volume">Volume to merge with</param>
        /// <returns>New volume that contains the original two</returns>
        public override BoundingVolume Merge(BoundingVolume volume) {
            if(volume == null) {
                return this.Clone();
            }

            float radius;
            Vector3 center;

            BoundingSphere result = new BoundingSphere();
            switch(volume.BoundingType) {
                case BoundingType.AABB:
                    BoundingBox box = volume as BoundingBox;
                    radius = box.Extents.Length();
                    center = box.Center;
                    MergeSphere(ref radius, ref center, result);
                    break;
                case BoundingType.Sphere:
                    BoundingSphere sphere = volume as BoundingSphere;
                    radius = sphere.Radius;
                    center = sphere.Center;
                    MergeSphere(ref radius, ref center, result);
                    break;
                case Bounding.BoundingType.OBB:
                    OrientedBoundingBox obb = volume as OrientedBoundingBox;
                    MergeOBB(obb, result);
                    break;
            }

            return result;
        }


        /// <summary>
        /// Handles the merge cases for AABB and other Spheres. Inputs are the
        /// radius and center of the volume to merge with this Sphere, and the
        /// merged volume is stored in the result.
        /// </summary>
        /// <param name="radius">Radius of the first volume</param>
        /// <param name="center">Center of the first volume</param>
        /// <param name="result">Sphere to store the result</param>
        private void MergeSphere(ref float radius, ref Vector3 center, BoundingSphere result) {
            Vector3 diff;
            Vector3.Subtract(ref center, ref m_center, out diff);
            float radDiff = radius - m_radius;
            float radDiffSqr = radDiff * radDiff;
            float lengthSquared = diff.LengthSquared();

            //Check if one sphere contains the other wholly
            if((radDiffSqr) >= lengthSquared) {
                //This sphere contains the other
                if(radDiff <= 0.0f) {
                    result.Set(this);
                    return;
                } 
                //Otherwise it contains us
                result.Center = center;
                result.Radius = radius;
                return;
            }

            //Find the new center between the two spheres
            Vector3 resCenter = m_center;
            float length = MathHelper.Sqrt(lengthSquared);

            //If the centers are at least a small amount apart, place the new center
            //between the two weighted by their radii
            if(length > MathHelper.Epsilon) {
                float a = (length + radDiff) / (2.0f * length);

                Vector3 temp;
                Vector3.Multiply(ref diff, a, out temp);
                Vector3.Add(ref resCenter, ref temp, out resCenter);
            }

            //Compute the new radius and set
            result.Radius = 0.5f * (length + m_radius + radius);
            result.Center = resCenter;
        }

        /// <summary>
        /// Handles the merge case for OBB. Input is the oriented bounding box,
        /// which is used with the properties of this Sphere to create a 
        /// merged volume that is stored in the result.
        /// </summary>
        /// <param name="obb">Oriented bounding box to merge with</param>
        /// <param name="result">Sphere to store the result</param>
        private void MergeOBB(OrientedBoundingBox obb, BoundingSphere result) {
            if(!obb.CorrectCorners) {
                obb.ComputeCorners();
            }

            //Save this sphere's radius/center (result sphere may be this sphere)
            float sRad = m_radius;
            Vector3 sCenter = m_center;

            //Use the input sphere and compute it to contain the OBB's corners
            result.ComputeFromPoints(obb.Corners);

            //Use our saved radius/center and merge the result sphere with this
            result.MergeSphere(ref sRad, ref sCenter, result);
        }

        /// <summary>
        /// Transforms this volume by the SRT transform. The transformed
        /// volume is stored locally.
        /// </summary>
        /// <param name="transform">SRT transform</param>
        public override void TransformLocal(Transform transform) {
            if(transform == null) {
                return;
            }

            //Transform the center
            Matrix m = transform.Matrix;
            Vector3.Transform(ref m_center, ref m, out m_center);

            //Find the max length along each axis to get the new radius 
            //to deal with scaling
            float x = m.Right.LengthSquared();
            float y = m.Up.LengthSquared();
            float z = m.Backward.LengthSquared();

            float max = MathHelper.Max(x, MathHelper.Max(y, z));

            m_radius = m_radius * MathHelper.Sqrt(max);
        }

        /// <summary>
        /// Transforms this volume by the SRT transform and creates a 
        /// new bounding volume that contains the result. The bounding volume
        /// will be of the same type as the caller.
        /// </summary>
        /// <param name="transform">SRT transform</param>
        /// <returns>New transformed volume</returns>
        public override BoundingVolume Transform(Transform transform) {
            if(transform == null) {
                return this.Clone();
            }

            Vector3 newCenter = m_center;
            float newRadius = m_radius;

            //Transform the center
            Matrix m = transform.Matrix;
            Vector3.Transform(ref newCenter, ref m, out newCenter);

            //Find the max axis to get the new radius
            float x = m.Right.LengthSquared();
            float y = m.Up.LengthSquared();
            float z = m.Backward.LengthSquared();

            float max = MathHelper.Max(x, MathHelper.Max(y, z));

            newRadius = newRadius * MathHelper.Sqrt(max);

            return new BoundingSphere(newCenter, newRadius);
        }

        /// <summary>
        /// Serializes this BoundingSphere.
        /// </summary>
        /// <param name="output">Output to write to</param>
        public override void Write(Content.ISavableWriter output) {
            base.Write(output);
            output.Write("Radius", m_radius);
        }

        /// <summary>
        /// Deserializes this BoundingSphere.
        /// </summary>
        /// <param name="input">Input to read from</param>
        public override void Read(ISavableReader input) {
            base.Read(input);
            m_radius = input.ReadSingle();
        }
    }
}
